/*
 * servos_motor.c
 *
 *  Created on: Apr 23, 2013
 *      Author: Daniel
 */

#include "DSP28x_Project.h"
#include "servos_motor.h"

void InitPWM() {
	InitPWM1();
	InitPWM2();
	InitPWM3();
	InitPWM4();
	InitPWM5();
	return;
}

void InitPWM1() {
	EALLOW;		// allows us to write to write-protected registers
	GpioCtrlRegs.GPAPUD.bit.GPIO0 = 1;	// disable pull-up on GPIO0 (reduces power consumption)
	GpioCtrlRegs.GPAMUX1.bit.GPIO0 = 1;	// Configure GPIO0 as EPWM1A

	EPwm1Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM1_TBPRD and then returns to 0
	EPwm1Regs.TBPRD = EPWM1_TBPRD;				// set timer period
	EPwm1Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm1Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm1Regs.TBCTR = 0x0000;					// clear counter
	EPwm1Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1;	// do not divide the clock input
	EPwm1Regs.TBCTL.bit.CLKDIV = TB_DIV1;		// do not divide the clock input

	// setup shadow register load on ZERO
	EPwm1Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm1Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm1Regs.CMPA.half.CMPA = EPWM1_CMPA;

	// set actions
	EPwm1Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM1A on counter = zero
	EPwm1Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM1A on counter = EPWM1_CMPA

	// disable interrupts
	EPwm1Regs.ETSEL.bit.INTEN = 0;

	// SYNCO should be synced to SYNCI (pass 3MHz signal through)
	// our working theory is to connect PWM1A to the EPWM_SYNCI pin on the MCU
	EPwm1Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_IN;

	// how to disable use of SYNCI??
}

void InitPWM2() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1;	// disable pull-up on GPIO2
	GpioCtrlRegs.GPAMUX1.bit.GPIO2 = 1; // Configure GPIO2 as EPWM2A
	GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1;	// disable pull-up on GPIO3
	GpioCtrlRegs.GPAMUX1.bit.GPIO3 = 1;	// Configure GPIO3 as EPWM2B

	EPwm2Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm2Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm2Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm2Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm2Regs.TBCTR = 0x0000;					// clear counter
	EPwm2Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1;	// do not divide the clock input
	EPwm2Regs.TBCTL.bit.CLKDIV = TB_DIV1;		// do not divide the clock input

	// setup shadow register load on ZERO
	EPwm2Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm2Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;
	EPwm2Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
	EPwm2Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm2Regs.CMPA.half.CMPA = EPWM2_CMPA_Default;
	EPwm2Regs.CMPB = EPWM2_CMPB_Off;

	// set actions
	EPwm2Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM2A on counter = zero
	EPwm2Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM2A on counter = EPWM2_CMPA
	EPwm2Regs.AQCTLB.bit.ZRO = AQ_SET;		// set PWM2B on counter = zero
	EPwm2Regs.AQCTLB.bit.CBU = AQ_CLEAR;	// clear PWM2B on counter = EPWM2_CMPB

	// disable interrupts
	EPwm2Regs.ETSEL.bit.INTEN = 0;

	// SYNCO should be synced to SYNCI (pass 3MHz signal through)
	EPwm2Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_IN;

	// how to enable use of SYNCI??
}

void InitPWM3() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO4 = 1;	// disable pull-up on GPIO4
	GpioCtrlRegs.GPAMUX1.bit.GPIO4 = 1;	// Configure GPIO4 as EPWM3A
	GpioCtrlRegs.GPAPUD.bit.GPIO5 = 1;	// disable pull-up on GPIO5
	GpioCtrlRegs.GPAMUX1.bit.GPIO5 = 1;	// Configure GPIO5 as EPWM3B

	EPwm3Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm3Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm3Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm3Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm3Regs.TBCTR = 0x0000;					// clear counter
	EPwm3Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1;	// do not divide the clock input
	EPwm3Regs.TBCTL.bit.CLKDIV = TB_DIV1;		// do not divide the clock input

	// setup shadow register load on ZERO
	EPwm3Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm3Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;
	EPwm3Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
	EPwm3Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm3Regs.CMPA.half.CMPA = EPWM3_CMPA_Default;
	EPwm3Regs.CMPB = EPWM3_CMPB_Default;

	// set actions
	EPwm3Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM3A on counter = zero
	EPwm3Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM3A on counter = EPWM3_CMPA
	EPwm3Regs.AQCTLB.bit.ZRO = AQ_SET;		// set PWM3B on counter = zero
	EPwm3Regs.AQCTLB.bit.CBU = AQ_CLEAR;	// clear PWM3B on counter = EPWM3_CMPB

	// disable interrupts
	EPwm3Regs.ETSEL.bit.INTEN = 0;

	// SYNCO should be synced to SYNCI (pass 3MHz signal through)
	EPwm3Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_IN;

	// how to enable use of SYNCI??
}

void InitPWM4() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO6 = 1;	// disable pull-up on GPIO6
	GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 1;	// Configure GPIO0 as EPWM4A

	EPwm4Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm4Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm4Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm4Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm4Regs.TBCTR = 0x0000;					// clear counter
	EPwm4Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1;	// do not divide the clock input
	EPwm4Regs.TBCTL.bit.CLKDIV = TB_DIV1;		// do not divide the clock input

	// setup shadow register load on ZERO
	EPwm4Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm4Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm4Regs.CMPA.half.CMPA = EPWM4_CMPA_Default;

	// set actions
	EPwm4Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM3A on counter = zero
	EPwm4Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM3A on counter = EPWM3_CMPA

	// disable interrupts
	EPwm4Regs.ETSEL.bit.INTEN = 0;

	// SYNCO should be synced to SYNCI (pass 3MHz signal through)
	EPwm4Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_IN;

	// how to enable use of SYNCI??
}

void InitPWM5() {
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO8 = 1;	// disable pull-up on GPIO8
	GpioCtrlRegs.GPAMUX1.bit.GPIO8 = 1;	// Configure GPIO0 as EPWM5A

	EPwm5Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;	// counter counts from 0 to EPWM2345_TBPRD and then returns to 0
	EPwm5Regs.TBPRD = EPWM2345_TBPRD;			// set timer period
	EPwm5Regs.TBCTL.bit.PHSEN = TB_DISABLE;		// disable phase loading
	EPwm5Regs.TBPHS.half.TBPHS = 0x0000;		// phase is 0
	EPwm5Regs.TBCTR = 0x0000;					// clear counter
	EPwm5Regs.TBCTL.bit.HSPCLKDIV = TB_DIV1;	// do not divide the clock input
	EPwm5Regs.TBCTL.bit.CLKDIV = TB_DIV1;		// do not divide the clock input

	// setup shadow register load on ZERO
	EPwm5Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW;
	EPwm5Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO;

	// set compare value
	EPwm5Regs.CMPA.half.CMPA = EPWM5_CMPA_Default;

	// set actions
	EPwm5Regs.AQCTLA.bit.ZRO = AQ_SET;		// set PWM5A on counter = zero
	EPwm5Regs.AQCTLA.bit.CAU = AQ_CLEAR;	// clear PWM5A on counter = EPWM5_CMPA

	// disable interrupts
	EPwm5Regs.ETSEL.bit.INTEN = 0;

	// SYNCO should be synced to SYNCI (pass 3MHz signal through)
	EPwm5Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_IN;

	// how to enable use of SYNCI??
}


